#include "Control.h"

// 定义一个全局 PID 变量
PID_Inc_TypeDef motor_pid;
PID_Pos_TypeDef Dir_pid;
//--
//  @brief      电机PID初始化（增量式）
//  @param      pid: PID结构体指针
//  @param      kp: 比例系数
//  @param      ki: 积分系数
//  @param      kd: 微分系数
//--
void MotorPID_Init(PID_Inc_TypeDef* pid, float kp, float ki, float kd)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    
    pid->error = 0.0f;
    pid->last_error = 0.0f;
    pid->prev_error = 0.0f;

    pid->delta_output = 0.0f;
    pid->output = 0.0f;
    pid->output_limit = 1000.0f;  // 默认限幅
}

//--
//  @brief      PID初始化（位置式）
//  @param      pid: PID结构体指针
//  @param      kp: 比例系数
//  @param      ki: 积分系数
//  @param      kd: 微分系数
//--
void PID_Position_Init(PID_Pos_TypeDef *pid, float kp, float ki, float kd)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->Ek = 0;
    pid->Ek_1 = 0;
    pid->Integral = 0;
    pid->Output = 0;
    pid->Output_max = 1000;
}
//--
//  @brief      速度环
//  @param      setpoint: 目标速度
//  @param      measured: 实际速度
//  @return     速度环输出
//--
int32_t Speed_Pid_Out(PID_Inc_TypeDef* pid, int32_t setpoint, int32_t measured)
{
    pid->error = setpoint - measured;

    // 增量计算 Δu(k) = Kp*(e(k)-e(k-1)) + Ki*e(k) + Kd*(e(k)-2e(k-1)+e(k-2))
    pid->delta_output = pid->Kp * pid->error;
    pid->Integral+=pid->error;
    if(pid->Integral>1000)
    pid->Integral=1000;
    else if(pid->Integral<-1000)
    pid->Integral=-1000;
    pid->output = pid->delta_output+pid->Integral*pid->Ki;
    // 限幅
    if (pid->output > pid->output_limit)
        pid->output = pid->output_limit;
    else if (pid->output < -pid->output_limit)
        pid->output = -pid->output_limit;

    return (int32_t)pid->output;
}


//--
//  @brief      位置环 - 位置式 PID 计算
//  @param      Target_Value: 目标位置
//  @param      Actual_Value: 实际位置
//  @return     位置环输出
//--
int32_t PID_Position_Calc(PID_Pos_TypeDef *pid, float Target_Value, float Actual_Value)
{
    pid->Ek = Target_Value - Actual_Value;
    float derivative = pid->Ek - pid->Ek_1;

    pid->Integral += pid->Ek;

    pid->Output = pid->Kp * pid->Ek + pid->Ki * pid->Integral + pid->Kd * derivative;

    if (pid->Output > pid->Output_max)
        pid->Output = pid->Output_max;
    else if (pid->Output < -pid->Output_max)
        pid->Output = -pid->Output_max;

    pid->Ek_1 = pid->Ek;
    return (int32_t)pid->Output;
}


int  L_speedcontrol(int ECPULSE, int aim_speed, float speed_p, float speed_i)
{
    static int speed_jifen = 0; // 积分项需保留状态，设为 static 保持上次值
    int out, Error;

    Error =  ECPULSE-aim_speed;
    speed_jifen += Error;

    // 防止积分饱和
    if (speed_jifen > 1000) speed_jifen = 1000;
    if (speed_jifen < -1000) speed_jifen = -1000;

    out = (int)(Error * speed_p + speed_jifen * speed_i);
    if (out > 4500) out = 4500;
    if (out < -4500) out = -4500;

    return out;
}

int R_speedcontrol(int ECPULSE, int aim_speed, float speed_p, float speed_i)
{
    static int speed_jifen = 0; // 积分项需保留状态，设为 static 保持上次值
    int out, Error;

    Error =  ECPULSE-aim_speed;
    speed_jifen += Error;

    // 防止积分饱和
    if (speed_jifen > 1000) speed_jifen = 1000;
    if (speed_jifen < -1000) speed_jifen = -1000;

    out = (int)(Error * speed_p + speed_jifen * speed_i);
    if (out > 4500) out = 4500;
    if (out < -4500) out = -4500;

    return out;
}

int32_t wh_Turn_Out(int16_t chazhi, float dir_p, float dir_d)
{
  float error;
  static float last_error = 0;
  int32_t Output;
  float error_derivative;

  error = chazhi;

  error_derivative = error - last_error;
  Output = error *dir_p + error_derivative * dir_d;
  last_error = error;
  return Output;
}